#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

#include <Eigen/Eigen>
#include <nav_msgs/Odometry.h>


Eigen::Vector3d pos_drone_t265;
Eigen::Quaterniond q_t265;


void t265_cb(const nav_msgs::Odometry::ConstPtr &msg)
{

    pos_drone_t265 = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
    q_t265 = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "t265_to_mavros");
    ros::NodeHandle nh;


    // 订阅t265估计位置
    ros::Subscriber t265_sub = nh.subscribe<nav_msgs::Odometry>("/iris_0/camera/odom/sample", 100, t265_cb);

    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/iris_0/mavros/vision_pose/pose", 10);

    // 设置节点休眠周期
    ros::Rate rate(30.0);

    while(ros::ok())
    {
        geometry_msgs::PoseStamped vision;

        vision.pose.position.x = pos_drone_t265[0];
        vision.pose.position.y = pos_drone_t265[1];
        vision.pose.position.z = pos_drone_t265[2];

        vision.pose.orientation.x = q_t265.x();
        vision.pose.orientation.y = q_t265.y();
        vision.pose.orientation.z = q_t265.z();
        vision.pose.orientation.w = q_t265.w();


        vision.header.stamp = ros::Time::now();
        vision_pub.publish(vision);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}
